// Fill out your copyright notice in the Description page of Project Settings.

#include "modules/map/hdmap/adapter/odr_parser/geometry_util/WorldSpiralIterator.h"
#include "modules/map/hdmap/adapter/odr_parser/geometry_util/odrSpiral.h"

namespace apollo {

FWorldSpiralIterator::FWorldSpiralIterator()
{}

FWorldSpiralIterator::FWorldSpiralIterator(double curvStart, double curvEnd, double length, double res)
{
	Reset(curvStart, curvEnd, length, res);
}

bool FWorldSpiralIterator::HasNext() const
{
	return !_itEnd;
}

void FWorldSpiralIterator::Next()
{
	if (_s >= _sEnd)
	{
		_itEnd = true;
	}
	else
	{
		_s = std::min(_s+_res, _sEnd);
		if (std::abs(_sEnd - _s) < _res * 0.0001) _s = _sEnd;
	}
}

void FWorldSpiralIterator::Reset(double curvStart, double curvEnd, double length, double res)
{
	_curvStart = curvStart;
	_curvEnd = curvEnd;
	_length = length;
	_res = res;

	_cDot = (curvEnd - curvStart) / length;
	_degenerate = (_cDot == 0 || length == 0);
	_sStart = curvStart / _cDot;
	_sEnd = curvEnd / _cDot;
	_s = _sStart;
	_x = _y = _t = 0.0;
	_itEnd = false;

	// first point is (0, 0). forward is +x after applying the align matrix.
	apollo_odrSpiral(_sStart, _cDot, &_x, &_y, &_tStart);
	_align = glm::dmat3x3(
		std::cos(-_tStart),	std::sin(-_tStart),	0.0,
		-std::sin(-_tStart),std::cos(-_tStart),	0.0,
		0.0,				0.0,				1.0
	);
	_align *= glm::dmat3x3(
		1.0,	0.0,	0.0,
		0.0,	1.0,	0.0,
		-_x,	-_y,	1.0
	);
}

glm::dvec3 FWorldSpiralIterator::Sample()
{
	apollo_odrSpiral(_s, _cDot, &_x, &_y, &_t);
	return _align * glm::dvec3(_x, _y, 1.0);
}

bool FWorldSpiralIterator::IsDegenerate() const
{
	return _degenerate;
}

double FWorldSpiralIterator::S() const
{
	return _s - _curvStart / _cDot;
}

} // namespace apollo